#ifndef SWEEPSURF_H_
#include "SweepSurf.h"
#endif

using namespace std;
using std::vector;

SweepSurf :: SweepSurf(vector<float>* Points,vector<float>* PathPoints){
 controlPoints = new vector<float>();
 controlPoints->insert(controlPoints->end(), Points->begin(), Points->end());

 vectorPerp[0]=0;
 vectorPerp[1]=0;
 vectorPerp[2]=1;

 delta[0]=0;
 delta[1]=0;
 delta[2]=0;

 BezierCuadCurve aux(PathPoints,vectorPerp);
 path = new vector<float>();
 aux.getCurve(path); 

 createSurface();
}

SweepSurf :: SweepSurf(){
 delete(controlPoints);
 delete(normals);
 delete(surface);
 delete(path);
}

void SweepSurf :: createSurface(){

 surface = new vector<float>();
 normals = new vector<float>();

 for(size_t i=3 ; i<path->size(); i+=3){
	
	float dx = (*path)[i]   - (*path)[i-3];
	float dy = (*path)[i+1] - (*path)[i-2];
	float dz = (*path)[i+2] - (*path)[i-1];

	if(i<path->size()-4){
		float dx2 = (*path)[i+1] - (*path)[i];
		float dy2 = (*path)[i+2] - (*path)[i+1];
		float dz2 = (*path)[i+3] - (*path)[i+2];

		float ang = calcularAngulo(dx,dy,dz,dx2,dy2,dz2);

		cout << "\nd: {" << dx << "," << dy << "," << dz << "}\n";
		cout << "\nd2: {" << dx2 << "," << dy2 << "," << dz2 << "}\n";
		cout << "\nang: " << ang*(180/3.14) << "\n\n";
		rotateCtrlPoints(ang);
	}

 	delta[0]+=dx;
 	delta[1]+=dy;
 	delta[2]+=dz;

	vector<float>* control = translate();
 	BezierCuadCurve curve(control,vectorPerp);
	largoCurva = curve.getSize();
	curve.getCurve(surface);
	curve.getNormals(normals);
	delete(control);
 }
}

vector<float>* SweepSurf :: translate(){
 vector<float>* control = new  vector<float>();

 for (size_t i=0; i<controlPoints->size();i+=3){
	(*control).push_back((*controlPoints)[i]+delta[0]);
	(*control).push_back((*controlPoints)[i+1]+delta[1]);
	(*control).push_back((*controlPoints)[i+2]+delta[2]);
 }

 return control;
}

float SweepSurf :: calcularAngulo(float dx,float dy,float dz,float dx2,float dy2,float dz2){
	float norm  = sqrt(dx*dx + dy*dy + dz*dz);
	float norm2 = sqrt(dx2*dx2 + dy2*dy2 + dz2*dz2);
	float prod  = dx*dx2 + dy*dy2 + dz*dz2;

	return acos(prod/(norm*norm2));
}

void SweepSurf :: rotateCtrlPoints(float angle){
 glm::mat4 rot = rotate(angle);

 for(size_t i=0 ; i<controlPoints->size() ; i+=3){
	glm::vec4 Position = transform(rot,(*controlPoints)[i],(*controlPoints)[i+1],(*controlPoints)[i+2]);
	(*controlPoints)[i]   = Position[0];
	(*controlPoints)[i+1] = Position[1];
	(*controlPoints)[i+2] = Position[2];
 }
}

// transforms the position using the transformation matrix
glm::vec4 SweepSurf :: transform(glm::mat4 transfMat,float x, float y, float z){

  glm::vec4 Position = glm::vec4(x,y,z,1.0f);
  return Position*transfMat;
}

// rotation matrix (x axis)
glm::mat4 SweepSurf :: rotate(float ang){
  glm::mat4  Rx = glm::mat4(glm::vec4(1.0f,                0.0f,                  0.0f, 0.0f),
			    glm::vec4(0.0f,     (float)cos(ang),      (float)-sin(ang), 0.0f),
			    glm::vec4(0.0f,     (float)sin(ang),       (float)cos(ang), 0.0f),
			    glm::vec4(0.0f,                0.0f,                  0.0f, 1.0f));
 return Rx;
}


void SweepSurf :: drawSurface(glm::mat4 mat){
 glPointSize(1.0);
 glBegin(GL_TRIANGLES);
 for (float t=0; t< surface->size()-2*largoCurva ;t+=3){	
		glm::vec4 Position = glm::vec4((*surface)[t],(*surface)[t+1],(*surface)[t+2],1.0f) * mat;
 		glVertex3f(Position[0],Position[1],Position[2]);

		Position = glm::vec4((*surface)[t+3],(*surface)[t+4],(*surface)[t+5],1.0f) * mat;
 		glVertex3f(Position[0],Position[1],Position[2]);

		Position = glm::vec4((*surface)[t+largoCurva],(*surface)[t+largoCurva+1],(*surface)[t+largoCurva+2],1.0f) * mat;
		glVertex3f(Position[0],Position[1],Position[2]);


		Position = glm::vec4((*surface)[t+largoCurva],(*surface)[t+largoCurva+1],(*surface)[t+largoCurva+2],1.0f) * mat;
		glVertex3f(Position[0],Position[1],Position[2]);

		Position = glm::vec4((*surface)[t+3],(*surface)[t+4],(*surface)[t+5],1.0f) * mat;
 		glVertex3f(Position[0],Position[1],Position[2]);

		Position = glm::vec4((*surface)[t+largoCurva+3],(*surface)[t+largoCurva+4],(*surface)[t+largoCurva+5],1.0f) * mat;
		glVertex3f(Position[0],Position[1],Position[2]);

 }
 glEnd();
}

void SweepSurf :: drawNormals(glm::mat4 mat){
 
 for (size_t i=0 ; i<surface->size()-3 ; i+=3){
		glBegin(GL_LINES);

		glm::vec4 Position = glm::vec4((*surface)[i],(*surface)[i+1],(*surface)[i+2],1.0) * mat;
 		glVertex3f(Position[0],Position[1],Position[2]);

		glm::vec4 Position2 = glm::vec4((*surface)[i]+(*normals)[i],(*surface)[i+1]+(*normals)[i+1],(*surface)[i+2]+(*normals)[i+2],1.0) * mat;
 		glVertex3f(Position2[0],Position2[1],Position2[2]);
	
		 glEnd();

		if( (i%largoCurva) == 0)
			i+= largoCurva*DETAILNORMALSSWEEPSURF;
  }
}

void SweepSurf :: drawCurve(glm::mat4 mat){
 BezierCuadCurve curve(controlPoints,vectorPerp);
 curve.drawCurve(mat);
 curve.drawNormals();
}

void SweepSurf :: drawPath(glm::mat4 mat){
 BezierCuadCurve curve(path,vectorPerp);
 curve.drawCurve(mat);
}

void SweepSurf :: drawControlPoints(){
 glBegin(GL_LINE_STRIP);
 for (float t=0; t< controlPoints->size() ;t+=3)	
		glVertex3f((*controlPoints)[t],(*controlPoints)[t+1],(*controlPoints)[t+2]);
 glEnd();
}
